/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-09-03 10:23:28
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-10-07 21:24:15
 */
#include <Arduino.h>
#include <IRremote.h>
#include <SoftwareSerial.h>
#include "RobotServoFw.h"
#include "MyOled.hpp"

// 红外遥控器键值定义机器人命令
#define RobotRun_CMD            0xB946FF00
#define RobotBack_CMD           0xEA15FF00
#define RobotLeft_CMD           0xBB44FF00
#define RobotRight_CMD          0xBC43FF00
#define RobotStand_CMD          0xBF40FF00
// #define RobotForward_CMD        0xE916FF00
// #define RobotBackward_CMD       0xE619FF00
#define Crouch_CMD        0xE916FF00


// 驱动板电源控制引脚
#define PowerPin                PD5
// 蜂鸣器控制引脚
#define Buzzer                  11
// 定义红外接收引脚
#define Recv_Pin                PD3
// 电池电量检测引脚
#define BAT_LEVEL_Pin           PIN_A0
#define WORK_VOLTAGE            (5)
#define BAT_MAX                 (2.1)
#define BAT_MIN                 (1.45)
#define BAT_FILTER              (10)
// 5V供电，分辨率1024；单位1024/5 = 0.0048
#define BAT_UNIT                (0.0048)
uint32_t Bat_level_value = 0;
// ADC均值滤波
uint32_t Bat_level_array[BAT_FILTER] = {0};
// 记录时间
uint32_t LastTimer = 0;


bool Robot_Crouch = false;      // 机器人下蹲状态，下蹲时不执行其它动作
RobotServoFw Robot = RobotServoFw(Serial);      // 机器人对象
SoftwareSerial mySerial(PD6, PD7);              // 开启模拟串口
MyOled *oled = new MyOled(0x3C);                // 创建一个OLED对象

/* 红外模块 */
IRrecv irrecv(Recv_Pin);    // 创建红外模块接收对象
uint32_t LastData = 0;      // 记录上一次读到的值，长按时调用

/* 状态更新检测 */
bool UpDataState = true;    // 状态更新时刷新显示


void setup() {
    // put your setup code here, to run once:
    /* 开启串口 */
    Serial.begin(115200);
    mySerial.begin(115200);
    /* 开启OLED */
    oled->Init();
    oled->ColorTurn(0);
    oled->DisplayTurn(0);
    oled->DrawBMP(41, 0, 46, 46, RobotLoGo);
    oled->ShowString(8, 6, "Tow Foot Robot", 16);
    /* 初始化GPIO */
    pinMode(PowerPin, OUTPUT);
    digitalWrite(PowerPin, HIGH);
    pinMode(Buzzer, OUTPUT);
    digitalWrite(Buzzer, HIGH);
    /* 开启红外接收 */
    irrecv.begin(Recv_Pin, false);
    /* 机器人复位 */
    Robot.Robot_Stand();
    /* 蜂鸣器响1000ms，提示开机 */
    delay(1000);
    digitalWrite(Buzzer, LOW);
    oled->Clear();
    mySerial.println("Irre Init!");
}

void loop() {
    // put your main code here, to run repeatedly:

    if(irrecv.decode())
    {
        uint8_t type = irrecv.decodedIRData.protocol;
        uint32_t RawData = irrecv.decodedIRData.decodedRawData;

        mySerial.print("protocol: ");
        mySerial.println(type, HEX);
        mySerial.print("decodedRawData: ");
        mySerial.println(RawData, HEX);

        if(type == 0 || type == 1 || type == 7)
        {
            UpDataState = true;
            if(RawData != 0)
            {
                LastData = RawData;
            }

            if (Robot_Crouch && LastData != RobotStand_CMD)
            {
                LastData = 0;
            }

            switch (LastData)
            {
            case RobotRun_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(52, 2, "RUN", 16);
                Robot.Robot_Running();
                break;

            case RobotBack_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(48, 2, "BACK", 16);
                Robot.Robot_Back();
                break;

            case RobotLeft_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(48, 2, "LEFT", 16);
                Robot.Robot_LeftHanded();
                break;

            case RobotRight_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(44, 2, "RIGHT", 16);
                Robot.Robot_RightHanded();
                break;

            case RobotStand_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(44, 2, "STAND", 16);
                Robot.Robot_Stand();
                Robot_Crouch = false;
                break;

            case Crouch_CMD:
                oled->Full(0, 2, 127, 16, 0);
                oled->ShowString(36, 2, "FORWARD", 16);
                Robot.Robot_Crouch();
                Robot_Crouch = true;
                break;

            // case RobotBackward_CMD:
            //     oled->Full(0, 2, 127, 16, 0);
            //     oled->ShowString(32, 2, "BACKWARD", 16);
            //     Robot.Robot_BackwardRoll();
            //     break;

            default:
                break;
            }
        }
        irrecv.resume();
    }
    else
    {
        if((millis() / 100) > LastTimer)
        {
            static uint8_t count = 0;
            LastTimer = millis() / 100;
            Bat_level_array[count++] = analogRead(BAT_LEVEL_Pin);

            if(count >= BAT_FILTER)
            {
                uint8_t i = 0;
                count = 0;
                Bat_level_value = 0;
                for(i=0; i<BAT_FILTER; i++)
                {
                    Bat_level_value += Bat_level_array[i];
                }
                Bat_level_value = Bat_level_value / BAT_FILTER;
                double RawVoltage = (double)Bat_level_value * BAT_UNIT;
                double NewVoltage = (RawVoltage > BAT_MIN) ? ((RawVoltage - BAT_MIN)) : (0);
                uint16_t BatPercent = (NewVoltage / (BAT_MAX - BAT_MIN)) * 100.0;
                oled->ShowNum(95, 0, BatPercent, 3, 16);
                oled->ShowChar(119, 0, '%', 16);
            }
        }
        if(UpDataState)
        {
            UpDataState = false;
            oled->Full(0, 2, 127, 16, 0);
            oled->ShowString(44, 2, "STATIC", 16);
        }
    }
}

